#include <sys/stat.h>
#include <sys/types.h>
#include <libfreenect.h>
#include <stdio.h>
#include <signal.h>
#include <string.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdexcept>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "MiscFunctions.h"
#include "motiondetection.h"
#include "FindRectTarget.h"
#include "ExtractLines.h"
#include "BackGroundPerPixel.h"
#include "BackGroundPerPixelDepth.h"
#include "BackGroundSingleFrame.h"
#include "CalibrationImageSelectorGUI.h"

using namespace cv;
using namespace std;
using namespace idch;

freenect_context *ctx = NULL;
freenect_device *dev = NULL;

int running = 0;

bool grabColor = false;
bool grabDepth = false;

cv::Mat colorImg;
cv::Mat depthImg;
uint32_t colorTS = 0;
uint32_t depthTS = 0;

bool saveImages = true;

#define MOTION_DEPTH_WINDOW "Motion Depth"
#define MOTION_RGB_WINDOW "Motion RGB"
#define ORIG_WINDOW "RGB"
#define DEPTH_WINDOW "Depth"
#define CONTOUR_RGB "Contour RGB"

void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp) {

    if( !grabColor )
        return;
    grabColor = false;

    uint8_t *dst = (uint8_t *)colorImg.datastart;
    const uint8_t *src = (const uint8_t *)rgb;

    int step = colorImg.step;
    int rowSize = sizeof(uint8_t)*FREENECT_FRAME_W*3;

    for( int i = 0; i < FREENECT_FRAME_H; i++ ) {

        memmove(dst,src,rowSize);
        dst += step;
        src += rowSize;
    }

    // change the time stamp at the end since this will inform it that
    // there is a new image
    colorTS = timestamp;
}

void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp) {

    if( !grabDepth )
        return;
    grabDepth = false;

    uint16_t *dst = (uint16_t *)depthImg.datastart;
    const uint16_t *src = (const uint16_t *)depth;

    int total = depthImg.datalimit-depthImg.datastart;
//    printf("imag size %d\n",total);

    int step = depthImg.step/2;
    int rowSize = sizeof(uint16_t)*FREENECT_FRAME_W;

//    printf("depth step = %d  row = %d\n",step,rowSize);

    for( int i = 0; i < FREENECT_FRAME_H; i++ ) {
//        if( (uint64_t)dst > (uint64_t)depthImg.datalimit)
//            throw std::runtime_error("dst exceeded bounds");
//        if( (uint64_t)src > (uint64_t)depth+(uint64_t)total)
//            throw std::runtime_error("src exceeded bounds");
        memmove(dst,src,rowSize);
        dst += step;
        src += rowSize/2;
//        printf("i = %d\n",i);
    }

    // change the time stamp at the end since this will inform it that
    // there is a new image
    depthTS = timestamp;
}

void signal_cleanup (int num) {
    running = 0;
    printf("Caught signal, cleaning up\n");
    signal(SIGINT, signal_cleanup);
}

void init_nect() {

    if (freenect_init(&ctx, 0)) {
        printf("Error: Cannot get context\n");
        return;
    }

    if (freenect_open_device(ctx, &dev, 0)) {
        printf("Error: Cannot get device\n");
        return;
    }
    freenect_set_depth_format(dev, FREENECT_DEPTH_11BIT);
    freenect_start_depth(dev);
    freenect_set_video_format(dev, FREENECT_VIDEO_RGB);
    freenect_start_video(dev);
    freenect_set_depth_callback(dev, depth_cb);
    freenect_set_video_callback(dev, rgb_cb);
}

void cleanup_nect() {
    freenect_stop_depth(dev);
    freenect_stop_video(dev);
    freenect_close_device(dev);
    freenect_shutdown(ctx);
}

void grabSingleColor( void ) {
    uint32_t oldTS = colorTS;
    grabColor = true;

    while( oldTS == colorTS ) {
        freenect_process_events(ctx);
        waitKey(10);
    }
}

void grabSingleDepth( void ) {
    uint32_t oldTS = depthTS;
    grabDepth = true;

    while( oldTS == depthTS ) {
        freenect_process_events(ctx);
        waitKey(10);
    }
}

void grabSingleAll( void ) {
    uint32_t oldColorTS = colorTS;
    uint32_t oldDepthTS = depthTS;
    grabColor = true;
    grabDepth = true;

    while( oldColorTS == colorTS || oldDepthTS == depthTS ) {
        freenect_process_events(ctx);
//        waitKey(10);
    }
}

void saveImage( const char *prefix , int num , cv::Mat &image )
{
    char temp[1024];

    IplImage a = image;
    sprintf(temp,"%s%05d.jpg",prefix,num);
    cvSaveImage(temp,&a);
}

int main( int argc , char **argv ) {
    printf("Initializing\n");

    // init OpenCV data structures
    cv::Mat gray = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC1);
    cv::Mat deriv = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC1);
    colorImg = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);
    depthImg = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_16UC1);
    cv::Mat contourRgbImg = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);
    cv::Mat contourDepthImg = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);

    cv::Mat grayRGB = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);

    FindRectTarget encoder(100,FREENECT_FRAME_W,FREENECT_FRAME_H);

//    namedWindow(MOTION_DEPTH_WINDOW);
//    namedWindow(MOTION_RGB_WINDOW);
//    namedWindow(ORIG_WINDOW);
//    namedWindow(DEPTH_WINDOW);
//    namedWindow(CONTOUR_RGB);

//    BackGroundPerPixel<uint8_t> bgGenRGB( 0.95 , 12 , 12 , FREENECT_FRAME_W , FREENECT_FRAME_H , 3 , 30 );
//    BackGroundPerPixelDepth bgGenDepth( 0.95, 2047 , FREENECT_FRAME_W , FREENECT_FRAME_H , 30 );

    BackGroundSingleFrame<uint8_t> bgGenRGB(35,FREENECT_FRAME_W , FREENECT_FRAME_H , 3 , false );
    BackGroundSingleFrame<uint16_t> bgGenDepth(10,FREENECT_FRAME_W , FREENECT_FRAME_H , 1 , true);

    MotionDetection motion(FREENECT_FRAME_W,FREENECT_FRAME_H,&bgGenRGB,&bgGenDepth);

    // Init the Kinect device
    init_nect();

    CalibrationImageSelectorGUI calibGUI(&motion,30);

    for( int imageNum = 0; ; imageNum++ ) {
        // grab the images
        grabSingleAll();

        // convert to gray scale
        cvtColor(colorImg, colorImg, CV_RGB2BGR);
//        cvtColor(colorImg, gray, CV_BGR2GRAY);
//        Laplacian(gray,deriv,8);
//        if( motion.addFrame(colorImg,depthImg) ) {

//        }

//        convertGray16to8(depthImg, gray, 0x07FF);

//        printf("Mode = %d  moving %d\n",motion.getMode(),motion.getNumMovingPixels());

//        imshow(ORIG_WINDOW,colorImg);
//        imshow(DEPTH_WINDOW,gray);
//        imshow(MOTION_DEPTH_WINDOW,motion.getMotionDepth());
//        imshow(MOTION_RGB_WINDOW,motion.getMotionRGB());


        calibGUI.process(colorImg,depthImg);

//        saveImage("saved/rgb",imageNum,colorImg);
//        saveImage("saved/depth",imageNum,gray);

//        encoder.process(motion.getMotionRGB());
//        contourRgbImg.setTo(Scalar(0,0,0));
//        encoder.drawContours(contourRgbImg);
//        encoder.drawCorners(contourRgbImg);
////        encoder.drawLines(contourRgbImg);
//        encoder.drawRectLineSeg(contourRgbImg);
//        imshow(MOTION_RGB_WINDOW,contourRgbImg);

////        saveImage("saved/contRgb",imageNum,contourRgbImg);

//        encoder.process(motion.getMotionDepth());
//        contourDepthImg.setTo(Scalar(0,0,0));
//        encoder.drawContours(contourDepthImg);
//        encoder.drawCorners(contourDepthImg);
////        encoder.drawLines(contourDepthImg);
//        encoder.drawRectLineSeg(contourDepthImg);
//        imshow(MOTION_DEPTH_WINDOW,contourDepthImg);

//        saveImage("saved/contDepth",imageNum,contourDepthImg);

//        imshow(CONTOUR_RGB,contourRgbImg);

        bool paused = false;

        for( int i = 0; i < 10 || paused ; i++ ) {
            freenect_process_events(ctx);
            int k = waitKey(10);
            if( k >= 0 ) {
                paused = !paused;
            }
        }
        printf("Got image!\n");
    }


}
